Trajectory planning

ABSTRACT

A method and apparatus are disclosed for determining a trajectory for a vehicle, wherein the method includes: identifying a starting position (p 0 ) for the vehicle; identifying a desired terminal position (P) for the vehicle; linearly approximating dynamics of the vehicle; and using the starting position (p 0 ), the desired terminal position (P), and the linear approximation, determining the trajectory for the vehicle. The linear approximation can be constrained by a requirement (e.g., specification) that a magnitude of a velocity of the vehicle is greater than or equal to a threshold value for the velocity. The vehicle may have a curvature limit.

FIELD OF THE INVENTION

The present invention relates to trajectory planning and apparatus forthe planning of trajectories for vehicles.

BACKGROUND

Trajectory planning methodologies, for example using Mixed IntegerLinear Programming (MILP), are used to determine globally optimaltrajectories for vehicles. Many trajectory planning methodologiesconstrain vehicle trajectories with a linear approximation of thevehicle's dynamics.

Typically, a linear approximation of the vehicle's dynamics does notcontain a notion of vehicle heading. This is typically because theintroduction of a heading angle introduces non-linearities. As a result,a determined trajectory may feature motion that cannot be achieved byconventional wheeled or tracked vehicles.

SUMMARY OF THE INVENTION

In a first aspect the present invention provides a method fordetermining a trajectory for a vehicle, the method comprising:identifying a starting position for the vehicle; identifying a desiredterminal position for the vehicle; linearly approximating dynamics ofthe vehicle; and using the starting position, the desired terminalposition, and the linear approximation, determining the trajectory forthe vehicle; wherein the linear approximation is constrained by arequirement that a magnitude of a velocity of the vehicle is greaterthan or equal to a threshold value for the velocity.

The constraint that a magnitude of the velocity of the vehicle isgreater than or equal to a threshold value for the velocity may beimplemented using the following:

∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circ)]${{{v_{x}(k)}{\sin \left( \frac{2\pi \; m}{N_{circ}} \right)}} + {{v_{y}(k)}{\cos \left( \frac{2\pi \; m}{N_{circ}} \right)}}} \geq {V_{m\; i\; n} - {{p\left( {k,m} \right)} \cdot C_{1}}}$and ∀k ∈ [0, …  , N_(steps)]${\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{circ} - 1}$

where:

-   -   v_(x)(k) is the velocity of the vehicle in an x-direction;    -   v_(y)(k) is the velocity of the vehicle in a y-direction;    -   V_(min) is the threshold velocity value;    -   N_(steps) is a number of time-steps for the trajectory;    -   N_(circ) is a number of corners of a polygon;    -   p(k, m) is a binary decision variable; and    -   C₁ is a constant.

C₁ may be relatively large compared to V_(min).

The linear approximation may be further constrained by requirementsthat: an acceleration applied to the vehicle at a point on thetrajectory is relatively large when the acceleration acts in a directionthat is substantially perpendicular to the velocity of the vehicle; andan acceleration applied to the vehicle at a point on the trajectory isrelatively small when the acceleration acts in a direction that issubstantially parallel to the velocity of the vehicle. This constraintmay be implemented using the following:

  ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circs)]${{\left( {\frac{{a_{x}(k)}R_{m\; i\; n}}{V_{{ma}\; x}} - \frac{\lambda \; {v_{x}(k)}}{V_{{ma}\; x}}} \right){\cos \left( \frac{2\pi \; m}{N_{circs}} \right)}} + {\left( {\frac{{a_{y}(k)}R_{m\; i\; n}}{V_{{ma}\; x}} - \frac{\lambda \; {v_{y}(k)}}{V_{m\; a\; x}}} \right){\sin \left( \frac{2\pi \; m}{N_{circs}} \right)}}} \leq \sqrt{1 + \lambda^{2}}$

where:

-   -   v_(x)(k) is the velocity of the vehicle in an x-direction;    -   v_(y)(k) is the velocity of the vehicle in a y-direction;    -   V_(max) is a maximum velocity of the vehicle;    -   N_(steps) is a number of time-steps for the trajectory;    -   N_(circs) is a number of corners of a polygon;    -   R_(min) is a minimum turn radius of the vehicle; and    -   λ is a constant.

The linear approximation may be further constrained by a requirementthat the vehicle may not travel in a given region; a centre point of theregion is at a distance from a predetermined point on the vehiclesubstantially equal to a minimum turn radius of the vehicle, in adirection substantially perpendicular to a velocity of the vehicle; anda distance from the centre point of the region to a point on a perimeterof the region is greater than or substantially equal to the minimum turnradius of the vehicle.

The constraint that the vehicle may not travel in a given region may beimplemented using the following:

  ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(regions)]${{\left( {{r_{x}(k)} - r_{0x} - {R_{m\; i\; n}v_{n\; x}}} \right){\cos \left( \frac{2\pi \; m}{N_{regions}} \right)}} + {\left( {{r_{y}(k)} - r_{0y} - {R_{m\; i\; n}v_{ny}}} \right){\sin \left( \frac{2\pi \; m}{N_{regions}} \right)}}} \geq {R_{m\; i\; n} - {C_{2}{p\left( {k,m} \right)}}}$  with   ∀k ∈ [0, …  , N_(steps)]$\mspace{20mu} {{\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{regions} - 1}}$

where:

-   -   v_(nx) is the normalised velocity of the vehicle in an        x-direction;    -   v_(ny) is the normalised velocity of the vehicle in a        y-direction;    -   r_(x)(k) is a position of the vehicle from an origin in the        x-direction;    -   r_(y)(k) is a position of the vehicle from an origin in the        y-direction;    -   r_(0x) is an initial position of the vehicle from an origin in        the x-direction;    -   r_(0y) is an initial position of the vehicle from an origin in        the y-direction;    -   N_(steps) is a number of time-steps for the trajectory;    -   N_(regions) is a number of corners of a polygon;    -   R_(min) is a minimum turn radius of the vehicle;    -   p(k, m) is a binary decision variable; and    -   C₂ is a constant.

The linear approximation may be further constrained by requirementsthat: an acceleration of the vehicle during the trajectory is less thana threshold value for the acceleration; and the threshold value for theacceleration is dependent on an infinity norm of a velocity of thevehicle. This constraint may be implemented using the following:

$\begin{matrix}{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{m \in \left\lbrack {1,\ldots \mspace{14mu},N_{cir}} \right\rbrack}}}{{{{a_{x}(k)}{\sin \left( \frac{2\pi \; m}{N_{cir}} \right)}} + {{a_{y}(k)}{\cos \left( \frac{2\pi \; m}{N_{cir}} \right)}}} \leq {a_{{ma}\; x}(k)}}} & (i) \\{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{p \in \left\lbrack {1,\ldots \mspace{14mu},N_{pol}} \right\rbrack}}}{{{{m(p)}{v_{x}(k)}} + {c(p)}} \geq {{a_{{ma}\; x}(p)} - {C_{L}b\; 1\left( {k,p} \right)}}}{{{{m(p)}{v_{y}(k)}} + {c(p)}} \geq {{a_{m\; {ax}}(p)} - {C_{L}b\; 2\left( {k,p} \right)}}}} & ({ii}) \\{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}}{{\sum\limits_{p = 1}^{N_{pol}}\left( {{b\; 1\left( {k,p} \right)} + {b\; 2\left( {k,p} \right)}} \right)} \leq {{2N_{pol}} - 1}}} & ({iii}) \\{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}}{{a_{{ma}\; x}(k)} \geq 0}} & ({iv})\end{matrix}$

where:

-   -   a_(max) is a maximum acceleration of the vehicle;    -   a_(x) is an acceleration of the vehicle in an x-direction;    -   a_(y) is an acceleration of the vehicle in a y-direction;    -   v_(x) is a velocity of the vehicle in an x-direction;    -   v_(y) is a velocity of the vehicle in a y-direction;    -   N_(steps) is a number of time-steps for the trajectory;    -   N_(cir) is a number of corners of a polygon;    -   N_(pol) is a number of linear functions, each linear function        relating the maximum acceleration of the vehicle to an infinity        norm of the velocity of the vehicle;    -   m(p) is a value of a gradient of a pth linear function;    -   c(p) is a value of pth linear function if the value of the        velocity of the vehicle is zero;    -   b1(k, p) and b2(k, p) are constants; and    -   C_(L) is a constant.

The method may further comprise: determining one or more further linearapproximations of the dynamics of the vehicle; and, using the one ormore further linear approximations, determining one or more furthertrajectories for the vehicle; wherein each of the one or more furthertrajectories is determined using a further linear approximation ofvehicle dynamics; and the trajectory and the one or more furthertrajectories are for implementation by the vehicle in series; each ofthe one or more further linear approximations is constrained by one ormore of the following: (i) a requirement that a magnitude of a velocityof the vehicle is greater than or equal to a threshold value for thevelocity; (ii) requirements that: an acceleration applied to the vehicleat a point on the trajectory is relatively large when the accelerationacts in a direction that is substantially perpendicular to the velocityof the vehicle; and an acceleration applied to the vehicle at a point onthe trajectory is relatively small when the acceleration acts in adirection that is substantially perpendicular to the velocity of thevehicle; (iii) a requirement that the vehicle may not travel in a givenregion; a centre point of the region is at a distance from apredetermined point on the vehicle substantially equal to a minimum turnradius of the vehicle, in a direction substantially perpendicular to avelocity of the vehicle; and a distance from the centre point of theregion to a point on a perimeter of the region is greater than orsubstantially equal to the minimum turn radius of the vehicle; and (iv)requirements that an acceleration of the vehicle during the trajectoryis less than a threshold value for the acceleration, and the thresholdvalue for the acceleration is dependent on an infinity norm of avelocity of the vehicle.

The vehicle may have a curvature limit for its trajectory.

The vehicle may be a land-based vehicle.

In a further aspect the present invention provides apparatus fordetermining a trajectory for a vehicle, the apparatus comprising one ormore processors arranged to: linearly approximate dynamics of thevehicle; and using an identified starting position for the vehicle, anidentified desired terminal position for the vehicle, and the linearapproximation, determine a trajectory for the vehicle; wherein thelinear approximation is constrained by a requirement that a magnitude ofa velocity of the vehicle is greater than or equal to a threshold valuefor the velocity.

In a further aspect the present invention provides a program orplurality of programs arranged such that when executed by a computersystem or one or more processors it/they cause the computer system orthe one or more processors to operate in accordance with the method ofany of the above aspects.

In a further aspect the present invention provides a machine readablestorage medium storing a program or at least one of the programsaccording to the above aspect of the invention.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a schematic illustration (not to scale) of a land basedvehicle for which an embodiment of a method of performing path planningis to be implemented;

FIG. 2 is a schematic illustration (not to scale) of a scenario in whichthe vehicle travels along a trajectory;

FIG. 3 is a schematic illustration (not to scale) representing avelocity space of the vehicle;

FIG. 4 is a process flow chart showing certain steps of a process ofdetermining a constrained acceleration vector for the vehicle;

FIG. 5 is a schematic illustration (not to scale) that is useful in theunderstanding of the process of FIG. 4;

FIG. 6 is a schematic illustration (not to scale) showing the vehicleand two regions into which the vehicle may not directly move; and

FIG. 7 is a schematic illustration (not to scale) of a graph showing thenon-linear relationship between a maximum acceleration of the vehicleand an ∞-norm of the velocity of the vehicle.

DETAILED DESCRIPTION

FIG. 1 is a schematic illustration (not to scale) of a land basedvehicle, hereinafter referred to as “the vehicle 2”, for which anembodiment of a trajectory planner (not shown) plans a trajectory. Theterminology “trajectory” is used herein to refer to a route of thevehicle from a first position to a second position.

The following information about a state and operation of the vehicle 2,described with reference to FIG. 1 and FIG. 2, will be useful inunderstanding the embodiment described later below.

The vehicle 2 has the following state vector x:

$x = \begin{pmatrix}r_{x} \\r_{y} \\\theta\end{pmatrix}$

where:

-   -   r_(x) is a distance of the vehicle 2 from an origin O in the        x-direction, as indicated in FIG. 1;    -   r_(y) is a distance of the vehicle 2 from the origin O in the        y-direction, as indicated in FIG. 1; and    -   θ is an angle between the x-axis and a heading of the vehicle,        hereinafter referred to as “the heading” and indicated in FIG. 1        by a solid arrow and the reference numeral 4.

Also,

$\overset{.}{x} = \begin{pmatrix}{v\; \sin \; \theta} \\{v\; \cos \; \theta} \\\omega\end{pmatrix}$

where:

-   -   {dot over (x)} is the time derivative of the state vector x;    -   v is a value of the speed of the vehicle that the vehicle        travels with (in the direction of the vehicle's heading 4); and    -   ω is the curvature of the vehicle's path.

In this embodiment, the vehicle 2 has the following input vector u:

$u = \begin{pmatrix}v \\\omega\end{pmatrix}$

The input vector u is constrained as follows:

∥v∥≦V _(max)

∥ω∥≦ω_(max)

where V_(max) and ω_(max) are maxima of the vehicle's speed and angularvelocity respectively.

In this embodiment, the state vector x of the vehicle 2 is linearised byreplacing the heading 4 with components of the speed v in the x- andy-directions, i.e.:

$x = \begin{pmatrix}r_{x} \\r_{y} \\{v\; \sin \; \theta} \\{v\; \cos \; \theta}\end{pmatrix}$

The dynamics of the system are given by a linear time-invariant systemof the form:

{dot over (x)}=Ax+Bu

Because the vehicle's speed v is part of the state vector x, thelinearised form of the input vector U comprises acceleration componentsin the x- and y-directions, i.e.

$u = \begin{pmatrix}a_{x} \\a_{y}\end{pmatrix}$

FIG. 2 is a schematic illustration (not to scale) of a scenario in whichthe vehicle 2 travels along a trajectory 6 (indicated by a dotted linein FIG. 2).

The trajectory 6 is divided into a series of points p₀, p₁, p₂, . . . ,p_(N). The ith point on the trajectory 6, i.e. p_(i), is a positionoccupied by the vehicle at an ith time-step.

State vectors of the vehicle 2 at each of the points p₀, . . . , p_(N)of the trajectory 6 are x(0), . . . , x(N) respectively. In other words,x(i) denotes the state of the vehicle 2 at the ith time-step. Also, aninitial state of the vehicle 2, i.e. a state of the vehicle 2 at thestart of the trajectory 6, is x₀.

A desired terminal position of the vehicle 2 is indicated in FIG. 2 bythe point P.

In this embodiment, the starting position p₀ and the desired terminalposition P are identified for the vehicle 2, for example by auser/operator of the vehicle 2, by any appropriate manner. As usedherein, the terminology “identified” includes any appropriate form ofidentifying, selecting, choosing, establishing, acquiring etc.

A state of the vehicle 2 at the desired terminal position P is X_(P).

Thus, it is desirable for the vehicle 2 to follow a trajectory 6 suchthat the distance d between the desired terminal position P, and theterminal position of the vehicle 2 after following the trajectory 6,i.e. the point p_(N), is minimised. In other embodiments, a ‘stage cost’is also minimised.

In this embodiment, such an optimal trajectory (a trajectory thatminimises the distance between P and p_(N)) is determined by atrajectory planner (not shown in the Figures).

In this embodiment, the determination of the optimal trajectory isimplemented using the following equation:

$\min\limits_{u,x}{g\left( {x,u} \right)}$

where the value function is given by

g(x,u)=∥[I0](x(N)−x _(P))∥

In other embodiments this may also include cost term associated withtraversing the trajectory, such as time or distance.

In this embodiment the optimisation problem is subject to the followingconstraints:

x(0)=x ₀

x(k+1)=Ax(k)+Bu(k)

([0I]x(k),u(k))εL

where:

-   -   g(x, u) is a value function that defines optimality;    -   I is a (2×2) identity matrix; and    -   L is a net of acceptable states (v_(x), v_(y)) and controls        (a_(x), a_(y)). Here, v_(x) is the speed of the vehicle 2 in the        x-direction, v_(y) is the speed of the vehicle 2 in the        y-direction, a_(x) is the acceleration of the vehicle 2 in the        x-direction, and a_(y) is the acceleration of the vehicle 2 in        the y-direction.

In this embodiment, L is used to constrain the magnitude of the speedand the acceleration of the vehicle 2.

Thus, the above equation provides that the Euclidean distance betweenx(N) and x_(p) is minimised (the velocity components of the statevector, x, are multiplied by 0 to remove them).

In this embodiment, the above described constraint equations are used.However, in other embodiments different constraint equations may be usedinstead of, or in addition to, some or all of the above constraintequations. For example, in other embodiments, constraint equations maybe used that provide that a control effort for the vehicle 2, or anumber of time-steps to reach the destination, is minimised.

The above equation for determining the optimal trajectory does not takeinto account the heading 4 of the vehicle 2. Thus, a trajectorydetermined using this equation alone may, at a certain point, includemotion that cannot be performed by the vehicle 2 used in thisembodiment. For example, the trajectory 6 determined as described abovemay require that, at a certain point, the vehicle 2 travels in adirection that is perpendicular to the vehicle's heading 4 at thatpoint. Such a trajectory cannot be followed by the vehicle 2 of thisembodiment (i.e. a land-based vehicle) because it would require aninfinitely large turn-rate.

Conventionally, this constraint on the vehicle's turn rate is treated asan acceleration constraint. “Receding Horizon Control In UnknownEnvironments: Experimental results”, Markus Deittert, Arthur Richards,and George Mathews, ICRA, Achorage, Ak., USA, May 2010, which isincorporated herein by reference, shows an implementation in which themagnitude of the vehicle's input vector u (i.e. an acceleration) islimited in relation to the vehicles maximum velocity, V_(max), such thata minimum turn radius, R_(min), is enforced, i.e.:

$a_{\max} = \frac{V_{\max}^{2}}{R_{\min}}$

Thus, conventionally, trajectories that include a turn having a turnradius of less than R_(min) tend to be avoided by the vehicle travellingat velocities close to its maximum, V_(max).

The conventional approach of transforming the vehicle's turn rate limitinto an acceleration constraint tends to fail at low speeds. Inparticular, if the vehicle slows down, the enforcement of

$a_{\max} = \frac{V_{\max}^{2}}{R_{\min}}$

tends to result in trajectories comprising turns with a turn radius lessthan R_(min).

In this embodiment, a minimum constraint to the vehicle's velocity isused. This constraint provides that the minimum speed of the vehicle 2,denoted hereinafter as “V_(min)”, is close to the maximum speed of thevehicle 2.

In this embodiment, this minimum speed constraint is enforced asfollows.

FIG. 3 is a schematic illustration representing a space of the vehicle'svelocity, hereinafter referred to as the “velocity plane 8”.

A convex polynomial approximation of a circle is indicated by thereference numeral 10 and is hereinafter referred to as the “polygon”.The polygon 10 approximates a circle having a radius equal to V_(min),indicated in FIG. 3 by an arrow and the reference numeral 12. Also, thepolygon 10 is centred at the origin O′ of the velocity plane 8.

In this embodiment, the vehicle's velocity vector at the kth time step,is:

v(k)=(v _(x) ,v _(y))^(T)

where:

-   -   v_(x) is the speed of the vehicle 2 in the x-direction at the        kth time-step; and    -   v_(y) is the speed of the vehicle 2 in the y-direction at the        kth time-step.

This vector v(k) is constrained to remain outside the polygon 10, i.e.

∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circ)]${{{v_{x}(k)}{\sin \left( \frac{2\; \pi \; m}{N_{circ}} \right)}} + {{v_{y}(k)}{\cos \left( \frac{2\; \pi \; m}{N_{circ}} \right)}}} \geq {V_{\min} - {{p\left( {k,m} \right)} \cdot C_{L}}}$with ∀k ∈ [0, …  , N_(steps)]${\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{circ} - 1}$

Where:

-   -   N_(steps) is the number of time-steps;    -   N_(circ) is the number of corners of the polygon 10;    -   p(k,m) is a binary decision variable. In this embodiment p(k,m)        is a matrix of size N_(steps)×N_(circ) with an entry (equal to        zero or one) for each time-step k and polygon corner m. In this        embodiment, p(k,m) is used to deactivate constraints; and    -   C_(L) is an arbitrary constant. In this embodiment C_(L) is        relatively large compared to V_(min).

The above velocity constraint is satisfied either by v(k) being outsidethe polygon 10, or by each value in the matrix p(k,m) being equal toone. The constraint

${\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{circ} - 1}$

provides that only N_(circ)−1 entries of p(k,m) are equal to one. Thus,v(k) must be outside of the polygon 10.

Thus, a technique by which a vehicle trajectory may be determined usinga trajectory planner is provided. The above described constraintsapplied to the trajectory planner, i.e. the constraints on the vector v,advantageously tend to provide that the determined trajectory is able tobe followed by a vehicle (e.g. a wheeled land vehicle) that has acurvature limit.

The above described embodiment, in which a constraint on the velocityvector v of the vehicle 2 is applied, advantageously tends to providethat the trajectory planner tends not to be able to produce trajectorieswhich require, at a particular point in time, the vehicle 2 to travelperpendicular to its heading 4.

A further advantage provided by the above described trajectory plannerconstraint is that the performance of the trajectory planner and/or thevehicle, in particular when the vehicle travels at relatively lowspeeds, tends to be improved compared to the conventional approach.Performance may, for example, be measured as the error between the turnradii of the optimal linear trajectory and the turn radius limit of thenon-linear vehicle dynamics.

A further advantage provided by the above described embodiment is that alinear approximation of the vehicle's dynamics is advantageouslyconstrained. In particular, in the above embodiment the magnitude of thevehicle acceleration in a direction perpendicular to the vehicle'sheading is constrained. This is achieved by constraining the velocityvector of the vehicle as described above. In other words, constraints onthe trajectory of the vehicle are implemented in the velocity space ofthe vehicle.

In other embodiments, the above described approach may be combined withone or more of the following additional optional techniques.

A first additional technique involves permitting an acceleration of thevehicle 2 that changes the direction of the vehicle 2, but that does notsignificantly change the norm of the vehicle's velocity.

This is achieved by requiring that the acceleration primarily acts indirections that are normal (i.e. perpendicular) to the velocity vectorv.

FIG. 4 is a process flow chart showing certain steps of a process ofdetermining a constrained acceleration vector for the vehicle 2according to the first additional technique.

FIG. 5 is a schematic illustration that is useful in the understandingof the process of FIG. 4.

At step s2, the velocity vector v of the vehicle 2 is normalised.

At step s4, the normalised velocity vector {circumflex over (v)} ismultiplied by a scalar quantity λ.

At step s6, two convex polynomial approximations of a circle,hereinafter referred to as “the first polygon” and the “second polygon”and indicated in FIG. 5 by the reference numerals 14 and 16respectively, are determined.

The first polygon 14 is centred at a point λ{circumflex over (v)}.

The second polygon 16 is centred at a point −λ{circumflex over (v)}.

The radii of the first and second polygons 14, 16 are indicated in FIG.5 by dotted arrows. These radii are larger than the length of the vectorλ{circumflex over (v)}. This provides that the first and second polygons14, 16 overlap to some extent.

At step s8, an acceleration vector a is determined such that it lieswithin an overlap 18 of the first polygon 14 with the second polygon.This provides that the acceleration applied to the vehicle 2 may berelatively large when acting in a direction that is substantiallyperpendicular to the velocity vector v of the vehicle 2, but isrelatively small when acting in a direction that is substantiallyparallel to the velocity vector v of the vehicle 2.

If A is selected to be a relatively large value, e.g. λ=10, theresulting overlap 18 is relatively small in a direction that issubstantially parallel to the velocity of the vehicle v, but isrelatively large in a direction that is substantially normal to thevelocity vector v. The value of λ may advantageously be selecteddepending on the application.

Due to the first and second polygons 14, 16 being centred around points,the position of which depends on the normalised velocity vector{circumflex over (v)}, if the vehicle 2 slows down, the overlap 18increases relative to the velocity vector v. This advantageously tendsto provide that, at low speeds, a relatively large acceleration may beapplied to the vehicle in a direction that is substantially parallel tothe velocity vector v, thereby allowing the magnitude of the velocityvector v to be increased.

The process of determining an acceleration vector for the vehicle 2described above with reference to FIGS. 4 and 5 is equivalent todetermining an acceleration vector

${a(k)} = \begin{pmatrix}a_{x} \\a_{y}\end{pmatrix}$

for the vehicle 2 that is constrained as follows:

  ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circs)]${{\left( {\frac{{a_{x}(k)}R_{\min}}{V_{\max}} - \frac{\lambda \; {v_{x}(k)}}{V_{\max}}} \right){\cos \left( \frac{2\; \pi \; m}{N_{circs}} \right)}} + {\left( {\frac{{a_{y}(k)}R_{\min}}{V_{\max}} - \frac{\lambda \; {v_{y}(k)}}{V_{\max}}} \right){\sin \left( \frac{2\; \pi \; m}{N_{circs}} \right)}}} \leq \sqrt{1 + \lambda^{2}}$

where: v(k)=(v_(x),v_(y))^(T); and

N_(circs) is the number of corners of the first and second polygons 14,16. In other embodiments, the first polygon 14 may have a differentnumber of corners to the second polygon 16 (and the above equation ismodified accordingly).

Thus, a first additional, optional technique by which a vehicletrajectory may be determined using a trajectory planner is provided. Theabove described constraints applied to the trajectory planner, i.e. theconstraints on the vector v, advantageously tend to provide that thedetermined trajectory is able to be followed by a vehicle (e.g. awheeled land vehicle) that has a curvature limit.

The above described embodiment, in which a constraint on theacceleration vector a of the vehicle 2 is applied, advantageously tendsto provide that the trajectory planner tends not to be able to producetrajectories which require, at a particular point in time, the vehicle 2to travel perpendicular to its heading 4.

A further advantage provided by the above described trajectory plannerconstraint is that the performance of the trajectory planner and/or thevehicle, in particular when the vehicle travels at relatively lowspeeds, tends to be improved compared to using the above describedvelocity constraint alone.

A further advantage provided by the above described embodiment is that alinear approximation of the vehicle's dynamics is advantageouslyconstrained. In particular, in the above embodiment the magnitude of thevehicle acceleration in a direction perpendicular to the vehicle'sheading is constrained. This is achieved by scaling the components ofthe acceleration vector (i.e. the components a_(x) and a_(y)) dependingof the respective components of the velocity vector (i.e. the componentsv_(x) and v_(y) respectively). In other words, constraints on thetrajectory of the vehicle are implemented in the acceleration space ofthe vehicle. In particular, the magnitude and the direction of thevehicle's acceleration is constrained.

A second additional technique involves constraining a trajectory plannersuch that it may not plan trajectories that require the vehicle 2 totravel in regions that limitations on the vehicle's turning circleprevent it from travelling in.

In particular, for curvature limited vehicles, which cannot turn on thespot, there exists a circular area to each side of the vehicle thatcannot be reached by turning directly into it, for example withoutrepeatedly reversing and advancing. The radius of these circles issubstantially equal to the vehicle's minimum turn radius, R_(min).

FIG. 6 is a schematic illustration showing the vehicle 2 and tworegions, hereinafter referred to as the “first region 20” and the“second region 22”, into which the vehicle 2 may not directly move, i.e.turn directly into.

The vehicle 2 may not move into the first or second region due tolimitations on the vehicle's curvature limit. The first and secondregion 20, 22 are avoided by the trajectory planner when planning atrajectory. The first and second regions 20, 22 may be considered to be‘obstacles’ that are to be avoided when planning a trajectory of thevehicle 2.

In embodiments in which the second additional technique is implemented,the first and second regions 20, 22 are approximated by polygons, andthe constraint on the trajectory of the vehicle 2 is implemented asfollows.

A unit vector normal to the initial speed vector of the vehicle v(0) is:

${\hat{v}}_{n} = {\left( {v_{nx},v_{ny}} \right)^{T} = \frac{\left( {{v_{y}(0)},{- {v_{x}(0)}}} \right)}{\sqrt{{v_{x}^{2}(0)} + {v_{y}^{2}(0)}}}}$

where:

-   -   v_(nx) the component of the vector {circumflex over (v)}_(n) in        the x-direction; and    -   v_(ny) is the component of the vector {circumflex over (v)}_(n)        in the y-direction.

An initial position of the vehicle 2 is given by:r₀=(r_(0x),r_(0y))^(T).

In embodiments in which the second additional technique is implemented,the constraint on the trajectory of the vehicle 2 supplied by one of theregion 20, 22 is implemented as follows:

  ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(regions)]${{\left( {{r_{x}(k)} - r_{0\; x} - {R_{\min}v_{nx}}} \right){\cos \left( \frac{2\; \pi \; m}{N_{regions}} \right)}} + {\left( {{r_{y}(k)} - r_{0\; y} - {R_{\min}v_{ny}}} \right){\sin \left( \frac{2\; \pi \; m}{N_{regions}} \right)}}} \geq {R_{\min} - {C_{L}{p\left( {k,m} \right)}}}$  ∀k ∈ [0, …  , N_(steps)]$\mspace{20mu} {{\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{regions} - 1}}$

where:

-   -   N_(steps) is the number of time-steps;    -   N_(regions) is the number of corners of the polygons used to        approximate the first or second regions 20, 22. In this        embodiment, the polygons used to approximate the first and        second regions 20, 22 comprise the same number of points.        However, in other embodiments these polygons may comprise a        different number of points. In such cases the above equations        may be modified accordingly;    -   p(k,m) is a binary decision variable. In this embodiment p(k,m)        is a matrix of size N_(steps)×N_(regions) with an entry (equal        to zero or one) for each time-step k and polygon corner m. In        this embodiment, p(k,m) is used to deactivate constraints; and

C_(L) is an arbitrary constant greater than R_(min). In this embodimentC_(L) is equal to 2×R_(min).

The constant C_(L) may be advantageously selected depending on theapplication.

Constraining the trajectory in this way advantageously tends to providethat vehicle 2 may follow the trajectory, even at relatively low speeds.This tends to be particularly useful when planning a trajectory from aresting position or in a cluttered surrounding.

Thus, a second additional, optional technique by which a vehicletrajectory may be determined using a trajectory planner is provided. Theabove described constraints applied to the trajectory planner, i.e. theconstraints on a position vector r(k)=(r_(x),r_(y)) of the vehicle,advantageously tend to provide that the determined trajectory is able tobe followed by a vehicle (e.g. a wheeled land vehicle) that has acurvature limit.

The above described embodiment, in which the trajectory is required toprovide that the vehicle 2 avoids regions close to, and either side of,the vehicle (as described above with reference to FIG. 6),advantageously tends to provide that the trajectory planner tends not tobe able to produce trajectories which require, at a particular point intime, the vehicle 2 to travel perpendicular to its heading 4.

A further advantage provided by the above described trajectory plannerconstraint is that the performance of the trajectory planner and/or thevehicle, in particular when the vehicle travels at relatively lowspeeds, tends to be improved compared to using the above describedvelocity constraint alone.

A further advantage provided by the above described embodiment is that alinear approximation of the vehicle's dynamics is advantageouslyconstrained. In particular, in the above embodiment the magnitude of thevehicle acceleration in a direction perpendicular to the vehicle'sheading is constrained. This is achieved by determining the componentsof the position vector (i.e. the components r_(x) and r_(y)), whichdefine a point that the vehicle will be moved to at a particular pointin time (in effect the heading of the vehicle at a point in time)depending of the respective components of the velocity vector (i.e. thecomponents v_(x) and v_(y) respectively). In other words, constraints onthe trajectory of the vehicle are implemented in the positional space(i.e. x-y space) of the vehicle.

A third additional technique involves linearly approximating the maximumacceleration of the vehicle 2.

The magnitude of the velocity of the vehicle 14 and maximum accelerationof the vehicle a_(max), are related by a nonlinear function.Conventionally, this non-linear function cannot be used directly in theoptimisation of the trajectory 6. However, in this embodiment thenonlinear function is approximated by a collection of linear functions.

In embodiments in which the third additional technique is implemented,the ∞-norm of the velocity of the vehicle v(k)=(v_(x),v_(y)) is used.This advantageously relates the amount of acceleration available to thetrajectory planner to the largest component within the speed vector, v.

FIG. 7 is a schematic illustration (not to scale) of a graph showing thenon-linear relationship between maximum acceleration of the vehiclea_(max), the ∞-norm of the velocity of the vehicle |v|, and the minimumturn radius R_(min). In FIG. 7 this non-linear function is indicated bythe reference numeral 24, and each of the collection of linear functionsthat are used to approximate the nonlinear function 24 are indicated bythe reference numeral 26.

In embodiments in which the third additional technique is implemented,the following constraints on the acceleration vector of the vehiclea(k)=(a_(x),a_(y))^(T) and the velocity vector of the vehiclev(k)=(v_(x),v_(y))^(T) are implemented:

$\begin{matrix}{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{m \in \left\lbrack {1,\ldots \mspace{14mu},N_{cir}} \right\rbrack}}}{{{{a_{x}(k)}{\sin \left( \frac{2\; \pi \; m}{N_{cir}} \right)}} + {{a_{y}(k)}{\cos \left( \frac{2\; \pi \; m}{N_{cir}} \right)}}} \leq {a_{\max}(k)}}} & (i) \\{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{p \in \left\lbrack {1,\ldots \mspace{14mu},N_{pol}} \right\rbrack}}}{{{{m(p)}{v_{x}(k)}} + {c(p)}} \geq {{a_{\max}(p)} - {C_{L}b\; 1\left( {k,p} \right)}}}{{{{m(p)}{v_{y}(k)}} + {c(p)}} \geq {{a_{\max}(p)} - {C_{L}b\; 2\left( {k,p} \right)}}}} & ({ii}) \\{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}}{{\sum\limits_{p = 1}^{N_{pol}}\left( {{b\; 1\left( {k,p} \right)} + {b\; 2\left( {k,p} \right)}} \right)} \leq {{2\; N_{pol}} - 1}}} & ({iii})\end{matrix}$

where:

-   -   N_(steps) is a number of time-steps;    -   N_(cir) is a number of corners of a polygon (a convex polynomial        approximation of a circle). This polygon approximates a circle        having a radius equal to a_(max);    -   N_(pol) is the number of linear functions 26 that are used to        approximate the nonlinear function 24;    -   m(p) is the value of the gradient of the pth linear function 26    -   c(p) is the value of the ‘a_(max)-intercept’ of the pth linear        function 26, i.e. the value of the pth linear function 26 if the        value of the velocity was zero;    -   b1(k, p) and b2(k, p) are constants the values of which provide        that at least one of the equations of the second constraint        (i.e. constraint (ii)) are fulfilled by a value of a_(max); and    -   C_(L) is a constant.

The first of the above constraints, i.e. constraint (i), provides thatthe magnitude of the acceleration vector a(k)=(a_(x),a_(y))^(T) islimited, i.e. that the magnitude of the acceleration vector is less thanor equal to a_(max) for a particular time-step.

The second of the above constraints, i.e. constraint (ii), provides thatthe maximum acceleration of the vehicle a_(max) is less than or equal tothe approximated non-linear function 24;

The third of the above constraints, i.e. constraint (iii), provides thata trajectory planner implementing the above constraints (i)-(iv) bases avalue of a_(max) on the x-component or y-component of the velocityvector v. The ∞-norm of the velocity vector v is thereby implemented.

The fourth of the above constraints, i.e. constraint (iv), provides thatthe maximum acceleration a_(max) is always positive.

Thus, a third additional, optional technique by which a vehicletrajectory may be determined using a trajectory planner is provided. Theabove described constraints applied to the trajectory planner, i.e. thatthe magnitude of acceleration of the vehicle (not the direction) isconstrained, advantageously tend to provide that the determinedtrajectory is able to be followed by a vehicle (e.g. a wheeled landvehicle) that has a curvature limit.

The above described embodiment, in which the trajectory is constrainedin such a way that inter alia the maximum value of the acceleration ofthe vehicle a_(max) is a function of either the x-component ory-component of the velocity vector v, advantageously tends to providethat the trajectory planner tends not to be able to produce trajectorieswhich require, at a particular point in time, the vehicle 2 to travelperpendicular to its heading 4.

A further advantage provided by the above described trajectory plannerconstraint is that the performance of the trajectory planner and/or thevehicle, in particular when the vehicle travels at relatively lowspeeds, tends to be improved compared to using the above describedvelocity constraint alone.

A further advantage provided by the above described embodiment is that alinear approximation of the vehicle's dynamics is advantageouslyconstrained. In particular, in the above embodiment the magnitude of thevehicle acceleration in a direction perpendicular to the vehicle'sheading is constrained. This is achieved by linear approximation of theacceleration limit of the vehicle (i.e. the maximum acceleration). Inother words, constraints on the trajectory of the vehicle areimplemented in the acceleration space of the vehicle. In particular, themagnitude of the maximum acceleration is constrained depending on thevelocity vector v.

One or more of the above described optional additional techniques forconstraining a trajectory determined by a trajectory planner mayadvantageously be implemented in conjunction with, or instead of, thevelocity constraints described above with reference to FIG. 3. It may bedesirable to use different sets of constraints in different situations.For example, in a case in which the vehicle starts with zero velocity,the acceleration limit constraint of the first additional techniquetends to be inappropriate (the vehicle is prevented from moving).However, when the vehicle is operating at high speed, this set ofconstraints tends to more desirable over the positional constraints ofthe second additional technique.

Different sets of constraints (i.e. the velocity constraints describedabove with respect to FIG. 3, or the constraints relating to any of theabove described additional technique) may be implemented by a trajectoryplanner contemporaneously or in series. How the constraints areimplemented, i.e. in what order and for what proportion of the vehicle'strajectory, may advantageously be selected depending on the application.For example, in another embodiment two sets of constraints may beapplied at different times: the positional constraints of the secondadditional technique may be implemented for a first half of a vehicle'strajectory, and the acceleration constraint of the first additionaltechnique may be implemented for the later half. In another embodiment,a point in time at which a constraint set being implemented by atrajectory planner is changed may be decided using a binary decisionvariable. In such a way, the change over from one set of constraints toanother can advantageously be tied to the vehicle's state.

Apparatus, including the trajectory planner (not shown in the Figures),for implementing the above arrangement, and performing any of the abovedescribed method steps, may be provided by configuring or adapting anysuitable apparatus, for example one or more computers or otherprocessing apparatus or processors, and/or providing additional modules.The apparatus may comprise a computer, a network of computers, or one ormore processors, for implementing instructions and using data, includinginstructions and data in the form of a computer program or plurality ofcomputer programs stored in or on a machine readable storage medium suchas computer memory, a computer disk, ROM, PROM etc., or any combinationof these or other storage media.

It should be noted that certain of the process steps depicted in theflowchart of FIG. 4 and described above may be omitted or such processsteps may be performed in differing order to that presented above andshown in FIG. 4. Furthermore, although all the process steps have, forconvenience and ease of understanding, been depicted as discretetemporally-sequential steps, nevertheless some of the process steps mayin fact be performed simultaneously or at least overlapping to someextent temporally.

In the above embodiments, the vehicle is a land-based vehicle, e.g. avehicle comprises wheels and/or tracks. However, in other embodimentsthe vehicle may be any appropriate vehicle that has a curvature limit,e.g. a boat, submarine, or amphibious vehicle. Also, the vehicle may bemanned or unmanned.

In the above embodiments, the constraints that are implemented by thetrajectory planner are expressed by the relevant above describedequations. In other embodiments, one or more of the constraints may beimplemented using a different appropriate equation so as to provide anequivalent constraining effect on the trajectory planner, and/or providethe equivalent functionality to that described above.

In the above embodiments, the polygons used in the linear approximationsmay comprise any appropriate number of corners. Generally, the greaterthe number of corners used for the polygon(s), the greater the accuracyof the approximation to the circles it/they represent tend to be.However, the greater the number of corners of the polygon(s), the moreprocessing power is required. Thus, a trade-off exists between accuracyand processing power. The number of corners for each of the polygonsused may be selected. A trade-off exists between the accuracy of theapproximation and the processing power required to perform theapproximation in a certain amount of time. The number of corners foreach polygon may be advantageously selected to achieve a desired balancebetween accuracy and processing power.

1. A method for determining a trajectory for a vehicle, the methodcomprising: identifying a starting position (p₀) for the vehicle;identifying a desired terminal position (P) for the vehicle; linearlyapproximating, by one or more processors, dynamics of the vehicle; anddetermining, based on the starting position (p₀), the desired terminalposition (P), and the linear approximation, by the one or moreprocessors, the trajectory for the vehicle; wherein the linearapproximation is constrained by a specification that a magnitude of avelocity of the vehicle is greater than or equal to a threshold valuefor the velocity.
 2. A method according to claim 1, wherein theconstraint that a magnitude of the velocity of the vehicle is greaterthan or equal to a threshold value for the velocity is implemented bycalculating: ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circ)]${{{v_{x}(k)}{\sin \left( \frac{2\; \pi \; m}{N_{circ}} \right)}} + {{v_{y}(k)}{\cos \left( \frac{2\; \pi \; m}{N_{circ}} \right)}}} \geq {V_{\min} - {{p\left( {k,m} \right)} \cdot C_{1}}}$and ∀k ∈ [0, …  , N_(steps)]${\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{circ} - 1}$where: v_(x)(k) is the velocity of the vehicle in an x-direction;v_(y)(k) is the velocity of the vehicle in a y-direction; V_(min) is thethreshold velocity value; N_(steps) is a number of time-steps for thetrajectory; N_(circ) is a number of corners of a polygon; p(k,m) is abinary decision variable; and C₁ is a constant.
 3. A method according toclaim 2, wherein C₁ is relatively large compared to V_(min).
 4. A methodaccording to claim 1, wherein the linear approximation is furtherconstrained by specifications, that: an acceleration applied to thevehicle at a point on the trajectory is relatively large when theacceleration acts in a direction that is substantially perpendicular tothe velocity of the vehicle; and an acceleration applied to the vehicleat a point on the trajectory is relatively small when the accelerationacts in a direction that is substantially parallel to the velocity ofthe vehicle.
 5. A method according to claim 4, wherein the constraintthat the acceleration is relatively large when acting in a directionthat is substantially perpendicular to the velocity of the vehicle, andis relatively small when acting in a direction that is substantiallyparallel to the velocity of the vehicle, is implemented using the bycalculating:   ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(circs)]${{\left( {\frac{{a_{x}(k)}R_{\min}}{V_{\max}} - \frac{\lambda \; {v_{x}(k)}}{V_{\max}}} \right){\cos \left( \frac{2\; \pi \; m}{N_{circs}} \right)}} + {\left( {\frac{{a_{y}(k)}R_{\min}}{V_{\max}} - \frac{\lambda \; {v_{y}(k)}}{V_{\max}}} \right){\sin \left( \frac{2\; \pi \; m}{N_{circs}} \right)}}} \leq \sqrt{1 + \lambda^{2}}$where: v_(x)(k) is the velocity of the vehicle in an x-direction;v_(y)(k) is the velocity of the vehicle in a y-direction; V_(max) is amaximum velocity of the vehicle; N_(steps) is a number of time-steps forthe trajectory; N_(circs) is a number of corners of a polygon; R_(min)is a minimum turn radius of the vehicle; and λ is a constant.
 6. Amethod according to claim 1, wherein: the linear approximation isfurther constrained by a specification that the vehicle not travel in agiven region; a centre point of the region is at a distance from apredetermined point on the vehicle substantially equal to a minimum turnradius of the vehicle, in a direction substantially perpendicular to avelocity of the vehicle; and a distance from the centre point of theregion to a point on a perimeter of the region is greater than orsubstantially equal to the minimum turn radius of the vehicle.
 7. Amethod according to claim 6, wherein the constraint that the vehicle nottravel in a given region is implemented by calculating:  ∀k ∈ [0, …  , N_(steps)], ∀m ∈ [1, …  , N_(regions)]${{\left( {{r_{x}(k)} - r_{0\; x} - {R_{\min}v_{nx}}} \right){\cos \left( \frac{2\; \pi \; m}{N_{regions}} \right)}} + {\left( {{r_{y}(k)} - r_{0\; y} - {R_{\min}v_{ny}}} \right){\sin \left( \frac{2\; \pi \; m}{N_{regions}} \right)}}} \geq {R_{\min} - {C_{2}{p\left( {k,m} \right)}}}$     with   ∀k ∈ [0, …  , N_(steps)]$\mspace{20mu} {{\sum\limits_{m = 1}^{N_{circ}}{p\left( {k,m} \right)}} = {N_{regions} - 1}}$where: v_(nx) is a normalised velocity of the vehicle in an x-direction;v_(ny) is a normalised velocity of the vehicle in a y-direction;r_(x)(k) is a position of the vehicle from an origin in the x-direction;r_(y)(k) is a position of the vehicle from an origin in the y-direction;r_(0x) is an initial position of the vehicle from an origin in thex-direction; r_(0y) is an initial position of the vehicle from an originin the y-direction; N_(steps) is a number of time-steps for thetrajectory; N_(regions) is a number of corners of a polygon; R_(min) isa minimum turn radius of the vehicle; p(k, m) is a binary decisionvariable; and C₂ is a constant.
 8. A method according to claim 1,wherein the linear approximation is further constrained byspecifications that: an acceleration of the vehicle during thetrajectory is less than a threshold value for the acceleration; and thethreshold value for the acceleration is dependent on an infinity norm ofa velocity of the vehicle.
 9. A method according to claim 8, wherein theconstraint that the linear approximation is constrained byspecifications that an acceleration of the vehicle during the trajectoryis less than a threshold value for the acceleration, and the thresholdvalue for the acceleration is dependent on an infinity norm of avelocity of the vehicle is implemented by calculating: $\begin{matrix}{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{m \in \left\lbrack {1,\ldots \mspace{14mu},N_{cir}} \right\rbrack}}}{{{{a_{x}(k)}{\sin \left( \frac{2\; \pi \; m}{N_{cir}} \right)}} + {{a_{y}(k)}{\cos \left( \frac{2\; \pi \; m}{N_{cir}} \right)}}} \leq {a_{\max}(k)}}} & (i) \\{{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}},{\forall{p \in \left\lbrack {1,\ldots \mspace{14mu},N_{pol}} \right\rbrack}}}{{{{m(p)}{v_{x}(k)}} + {c(p)}} \geq {{a_{\max}(p)} - {C_{L}b\; 1\left( {k,p} \right)}}}{{{{m(p)}{v_{y}(k)}} + {c(p)}} \geq {{a_{\max}(p)} - {C_{L}b\; 2\left( {k,p} \right)}}}} & ({ii}) \\{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}}{{\sum\limits_{p = 1}^{N_{pol}}\left( {{b\; 1\left( {k,p} \right)} + {b\; 2\left( {k,p} \right)}} \right)} \leq {{2\; N_{pol}} - 1}}} & ({iii}) \\{{\forall{k \in \left\lbrack {0,\ldots \mspace{14mu},N_{steps}} \right\rbrack}}{{a_{\max}(k)} \geq 0}} & ({iv})\end{matrix}$ where: a_(max) is a maximum acceleration of the vehicle;a_(x) is an acceleration of the vehicle in an x-direction; a_(y) is anacceleration of the vehicle in a y-direction; v_(x) is a velocity of thevehicle in an x-direction; v_(y) is a velocity of the vehicle in ay-direction; N_(steps) is a number of time-steps for the trajectory;N_(cir) is a number of corners of a polygon; N_(pol) is a number oflinear functions, each linear function relating the maximum accelerationof the vehicle to an infinity norm of the velocity of the vehicle; m(p)is a value of a gradient of a pth linear function; c(p) is a value ofpth linear function if the value of the velocity of the vehicle is zero;b1(k, p) and b2(k, p) are constants; and C_(L) is a constant.
 10. Amethod according to claim 1, comprising: determining, by the one or moreprocessors, one or more further linear approximations of the dynamics ofthe vehicle; and determining, based on the one or more further linearapproximations, by the one or more processors, one or more furthertrajectories for the vehicle; wherein the trajectory and the one or morefurther trajectories are for implementation by the vehicle in series;each of the one or more further linear approximations is constrained byone or more of the following: (i) a specification that a magnitude of avelocity of the vehicle is greater than or equal to a threshold valuefor the velocity; (ii) specification that: an acceleration applied tothe vehicle at a point on the trajectory is relatively large when theacceleration acts in a direction that is substantially perpendicular tothe velocity of the vehicle; and an acceleration applied to the vehicleat a point on the trajectory is relatively small when the accelerationacts in a direction that is substantially perpendicular to the velocityof the vehicle; (iii) a specification that the vehicle not travel in agiven region; a centre point of the region is at a distance from apredetermined point on the vehicle substantially equal to a minimum turnradius of the vehicle, in a direction substantially perpendicular to avelocity of the vehicle; and a distance from the centre point of theregion to a point on a perimeter of the region is greater than orsubstantially equal to the minimum turn radius of the vehicle; and (iv)specification that an acceleration of the vehicle during the trajectoryis less than a threshold value for the acceleration, and the thresholdvalue for the acceleration is dependent on an infinity norm of avelocity of the vehicle.
 11. A method according to claim 1, wherein thevehicle has a curvature limit for its trajectory.
 12. A method accordingto claim 1, wherein the vehicle is a land-based vehicle.
 13. Apparatusfor determining a trajectory for a vehicle, the apparatus comprising oneor more processors and associated memory which, in combination, areconfigured to: linearly approximate dynamics of the vehicle; anddetermine, based on an identified starting position (p₀) for thevehicle, an identified desired terminal position (P) for the vehicle,and the linear approximation, a trajectory for the vehicle; wherein thelinear approximation is constrained by a specification that a magnitudeof a velocity of the vehicle is greater than or equal to a thresholdvalue for the velocity.
 14. A program or plurality of programs stored ina non-transitory medium, in combination with a computer system or one ormore processors, such that when executed by a computer system or one ormore processors, the program or programs will cause the computer systemor the one or more processors to operate in accordance with the methodof claim
 1. 15. A machine readable non-transitory storage medium storinga program in machine readable form for causing a computer, uponexecution by the computer, to perform a method according to claim 1.